import json
from typing import Dict, Any, Tuple

try:
    from gglobal import logger
except ImportError:
    import logging
    logger = logging.getLogger(__name__)


def process_baselink_tf_to_pose(baselink_data: Dict[str, Any]) -> Dict[str, Any]:
    """
    将/baselink_tf数据转换为PoseStamped格式
    
    Args:
        baselink_data: baselink_tf数据字典
        
    Returns:
        PoseStamped格式的数据字典
    """
    try:
        pose_stamped = {
            "header": {
                "seq": 0,
                "stamp": {
                    "secs": 0,
                    "nsecs": 0
                },
                "frame_id": "map"
            },
            "pose": {
                "position": {
                    "x": baselink_data["position"]["x"],
                    "y": baselink_data["position"]["y"],
                    # "z": baselink_data["position"]["z"]
                    "z": 0
                },
                "orientation": {
                    "x": baselink_data["orientation"]["x"],
                    "y": baselink_data["orientation"]["y"],
                    "z": baselink_data["orientation"]["z"],
                    "w": baselink_data["orientation"]["w"]
                }
            }
        }
        
        logger.debug(f"已转换baselink_tf为PoseStamped格式")
        return pose_stamped
        
    except Exception as e:
        logger.error(f"转换baselink_tf为PoseStamped时出错：{e}", exc_info=True)
        raise


def process_baselink_tf_to_pointstamped(baselink_data: Dict[str, Any]) -> Dict[str, Any]:
    """
    将/baselink_tf数据转换为PointStamped格式
    
    Args:
        baselink_data: baselink_tf数据字典
        
    Returns:
        PointStamped格式的数据字典
    """
    try:
        point_stamped = {
            "header": {
                "seq": 0,
                "stamp": {
                    "secs": baselink_data.get("header", {}).get("stamp", {}).get("secs", 0),
                    "nsecs": baselink_data.get("header", {}).get("stamp", {}).get("nsecs", 0)
                },
                "frame_id": "map"  # 或者从baselink_data的header中获取frame_id
            },
            "point": {
                "x": baselink_data["position"]["x"],
                "y": baselink_data["position"]["y"],
                # "z": baselink_data["position"]["z"]
                "z": 0
            }
        }
        
        logger.debug(f"已转换baselink_tf为PointStamped格式")
        return point_stamped
        
    except Exception as e:
        logger.error(f"转换baselink_tf为PointStamped时出错：{e}", exc_info=True)
        raise


def process_baselink_tf_to_marker(baselink_data: Dict[str, Any]) -> Dict[str, Any]:
    """
    将/baselink_tf数据转换为Marker格式，生成一个以baselink_tf点为中心的矩形
    矩形规格：正方向2，反方向0.5，左右各0.5
    使用LINE_STRIP类型，通过四个顶角点组成矩形
    
    Args:
        baselink_data: baselink_tf数据字典
        
    Returns:
        Marker格式的数据字典
    """
    import math
    
    try:
        # 获取baselink位置
        pos_x = baselink_data["position"]["x"]
        pos_y = baselink_data["position"]["y"]
        pos_z = baselink_data["position"]["z"]
        
        # 获取朝向四元数
        qx = baselink_data["orientation"]["x"]
        qy = baselink_data["orientation"]["y"]
        qz = baselink_data["orientation"]["z"]
        qw = baselink_data["orientation"]["w"]
        
        # 将四元数转换为yaw角度（绕Z轴旋转）
        yaw = math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
        # 根据实际坐标系定义，修正yaw角，解决90度偏差问题
        yaw -= math.pi / 2.0
        
        # 计算矩形的四个顶角点（相对于baselink中心）
        # 正方向距离：2，反方向距离：0.5，侧边距离：0.5
        front_dist = 1.95
        back_dist = 0.23
        side_dist = 0.5
        
        # 在局部坐标系中定义四个顶角点
        local_points = [
            (-side_dist, front_dist),   # 左上角
            (side_dist, front_dist),    # 右上角
            (side_dist, -back_dist),    # 右下角
            (-side_dist, -back_dist),   # 左下角
            (-side_dist, front_dist)    # 回到左上角，闭合矩形
        ]
        
        # 将局部坐标转换为全局坐标
        cos_yaw = math.cos(yaw)
        sin_yaw = math.sin(yaw)
        
        global_points = []
        for local_x, local_y in local_points:
            # 旋转变换
            global_x = pos_x + local_x * cos_yaw - local_y * sin_yaw
            global_y = pos_y + local_x * sin_yaw + local_y * cos_yaw
            global_z = 0  # 矩形高度为0，保持在baselink的Z高度
            
            global_points.append({
                "x": global_x,
                "y": global_y,
                "z": global_z
            })
        
        # 创建LINE_STRIP类型的marker
        marker = {
            "header": {
                "seq": 0,
                "stamp": {
                    "secs": 0,
                    "nsecs": 0
                },
                "frame_id": "map"
            },
            "ns": "baselink_tf",
            "id": 0,
            "type": 4,  # LINE_STRIP类型
            "action": 0,  # ADD
            "pose": {
                "position": {
                    "x": 0.0,
                    "y": 0.0,
                    "z": 0.0
                },
                "orientation": {
                    "x": 0.0,
                    "y": 0.0,
                    "z": 0.0,
                    "w": 1.0
                }
            },
            "scale": {
                "x": 0.1,  # 线条宽度
                "y": 0.0,
                "z": 0.0
            },
            "color": {
                "r": 1.0,
                "g": 1.0,
                "b": 0.0,
                "a": 0.8
            },
            "lifetime": {
                "secs": 0,
                "nsecs": 0
            },
            "frame_locked": False,
            "points": global_points,
            "colors": [],
            "text": "",
            "mesh_resource": "",
            "mesh_use_embedded_materials": False
        }
        
        logger.debug(f"已转换baselink_tf为Marker格式")
        return marker
        
    except Exception as e:
        logger.error(f"转换baselink_tf为Marker时出错：{e}", exc_info=True)
        raise


def process_baselink_tf_to_vehicle_marker(baselink_data: Dict[str, Any]) -> Dict[str, Any]:
    """
    将/baselink_tf数据转换为车辆模型Marker格式，使用MESH_RESOURCE类型
    根据config.get_param("vehicle", "")获取车辆类型，加载对应的STL文件
    
    Args:
        baselink_data: baselink_tf数据字典
        
    Returns:
        Marker格式的数据字典，如果STL文件不存在则返回None
    """
    import os
    
    try:
        # 获取车辆类型参数
        from gglobal import config
        vehicle_type = config.get_param("vehicle", "")
        
        if not vehicle_type:
            logger.debug("vehicle参数为空，跳过车辆模型Marker生成")
            return None
        
        # 构造STL文件路径
        stl_file_path = f"apis/ros1/vehicle/{vehicle_type}.stl"
        
        # 检查STL文件是否存在
        if not os.path.exists(stl_file_path):
            logger.debug(f"STL文件不存在: {stl_file_path}，跳过车辆模型Marker生成")
            return None
        
        # 获取baselink位置和朝向
        pos_x = baselink_data["position"]["x"]
        pos_y = baselink_data["position"]["y"]
        pos_z = baselink_data["position"]["z"]
        
        # 原始四元数
        qx = baselink_data["orientation"]["x"]
        qy = baselink_data["orientation"]["y"]
        qz = baselink_data["orientation"]["z"]
        qw = baselink_data["orientation"]["w"]
        
        # 沿x轴旋转-90°的四元数 (sin(-45°)=-sin(45°), cos(-45°)=cos(45°))
        import math
        rot_x_90 = {
            "x": -math.sin(math.pi/4),  # -0.7071
            "y": 0.0,
            "z": 0.0,
            "w": math.cos(math.pi/4)    # 0.7071
        }
        
        # 四元数乘法：q_result = q_original * q_rotation
        # q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2) + 
        #           (w1*x2 + x1*w2 + y1*z2 - z1*y2)i +
        #           (w1*y2 - x1*z2 + y1*w2 + z1*x2)j +
        #           (w1*z2 + x1*y2 - y1*x2 + z1*w2)k
        final_qw = qw * rot_x_90["w"] - qx * rot_x_90["x"] - qy * rot_x_90["y"] - qz * rot_x_90["z"]
        final_qx = qw * rot_x_90["x"] + qx * rot_x_90["w"] + qy * rot_x_90["z"] - qz * rot_x_90["y"]
        final_qy = qw * rot_x_90["y"] - qx * rot_x_90["z"] + qy * rot_x_90["w"] + qz * rot_x_90["x"]
        final_qz = qw * rot_x_90["z"] + qx * rot_x_90["y"] - qy * rot_x_90["x"] + qz * rot_x_90["w"]
        
        # 创建MESH_RESOURCE类型的marker
        marker = {
            "header": {
                "seq": 0,
                "stamp": {
                    "secs": 0,
                    "nsecs": 0
                },
                "frame_id": "map"
            },
            "ns": "vehicle_model",
            "id": 1,
            "type": 10,  # MESH_RESOURCE类型
            "action": 0,  # ADD
            "pose": {
                "position": {
                    "x": pos_x,
                    "y": pos_y,
                    "z": 0.0  # 设置为地面高度
                },
                "orientation": {
                    "x": final_qx,
                    "y": final_qy,
                    "z": final_qz,
                    "w": final_qw
                }
            },
            "scale": {
                "x": 1.0,
                "y": 1.0,
                "z": 1.0
            },
            "color": {
                "r": 1.0,
                "g": 0.9,
                "b": 0.88,
                "a": 0.8
            },
            "lifetime": {
                "secs": 0,
                "nsecs": 0
            },
            "frame_locked": False,
            "points": [],
            "colors": [],
            "text": "",
            "mesh_resource": f"file://{os.path.abspath(stl_file_path)}",
            "mesh_use_embedded_materials": False
        }
        
        logger.debug(f"已生成车辆模型Marker: {vehicle_type}")
        return marker
        
    except Exception as e:
        logger.error(f"生成车辆模型Marker时出错：{e}", exc_info=True)
        return None